﻿using System;
using System.IO.Ports;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SecretLabs.NETMF.Hardware;
using SecretLabs.NETMF.Hardware.Netduino;


namespace INetduinoServos
{
    public class Program
    {

        private static void PWM_WriteServo_TinyS(PWM pwmServo, Byte iDegree)
        {
            //Define the max, neutral and min pulse dyal servo Tiny-S
            uint iMaxPulse = 2400; //en us
            uint iMinPulse = 600;
            uint iMoyPulse = 1500;

            uint iPos;
            iPos = (uint)((((uint)iDegree) * (iMaxPulse - iMinPulse)) / 180) + iMinPulse;
            if (iPos > iMaxPulse) iPos = iMaxPulse;
            if (iPos < iMinPulse) iPos = iMinPulse;
            pwmServo.SetPulse(20000, iPos);//The period is 20000us = 20ms and the pulse is between 1ms and 2ms
        }

        public static void Main()
        {
            // Inisializi l'port siri  
            SerialPort mySerial1 = new SerialPort("COM1", 38400, Parity.None, 8, StopBits.One);
            mySerial1.Open();

            

            Boolean m_bIsBufferOK = false;
            Byte m_iSeparationByte = 255;
            Byte m_iBufferSize = 6;
            Byte[] m_RxBuffer = new Byte[m_iBufferSize];

            Byte m_iEchoMsg = 1;
            
            PWM m_PWMPuissanceMoteur    = new PWM(Pins.GPIO_PIN_D5);//Pin5 PWM: Puissance dyal l'moteur
            PWM m_PWMDerive             = new PWM(Pins.GPIO_PIN_D6);//Pin6 PWM: Servo dyal d'dérive
            PWM m_PWMProfondeur         = new PWM(Pins.GPIO_PIN_D9);//Pin9 PWM: Servo dyal l'profondeur

            while (true)
            {
                int iNumberOfBytes = mySerial1.BytesToRead;

                /*  //Echo test
                  if(iNumberOfBytes>0){
                  iReceivedByte1 = Serial.read();
                  Serial.print(iReceivedByte1);
                }*/

                //Receive the buffer
                if (iNumberOfBytes >= m_iBufferSize)
                {

                    //Check that there is a separation
                    m_RxBuffer[0] = 0;
                    mySerial1.Read(m_RxBuffer,0,1);
                    m_bIsBufferOK = false;

                    if (m_RxBuffer[0] == m_iSeparationByte)
                    {
                        //We got a separation byte, we read the next part of the message
                        mySerial1.Read(m_RxBuffer, 1, m_iBufferSize - 1);
                        //m_RxBuffer[1] = Type de commande
                        //m_RxBuffer[2] = Puissance Moteur
                        //m_RxBuffer[3] = Dérive
                        //m_RxBuffer[4] = Profondeur
                        //m_RxBuffer[5] = CheckSum   

                        //Check that the received message is coherent
                        //TODO: create a check sum method
                        m_bIsBufferOK = true;

                        if (m_bIsBufferOK)
                        {

                            //Send commands to Servos
                            /*uint iPos;
                            iPos = (uint)((((uint)m_RxBuffer[3]) * (2000 - 1000)) / 180) + 1000;
                            if (iPos > 2000) iPos = 2000;
                            if (iPos < 1000) iPos = 1000;
                            m_PWMPuissanceMoteur.SetPulse(20*1000, iPos);//The period is 20000us = 20ms and the pulse is between 1ms and 2ms*/

                            PWM_WriteServo_TinyS(m_PWMPuissanceMoteur, m_RxBuffer[2]);
                            PWM_WriteServo_TinyS(m_PWMDerive, m_RxBuffer[3]);
                            PWM_WriteServo_TinyS(m_PWMProfondeur, m_RxBuffer[4]);
                            
                            //Send back the message
                            if (m_RxBuffer[1] == m_iEchoMsg)
                            {
                                //Sending back echo - To send upon request 
                                mySerial1.Write(m_RxBuffer,0,m_iBufferSize);
                            
                            }//End sending back message

                        }//End we got a valid message

                    }//End we got a message with separation

                    //Thread.Sleep(20);
                }//End control and feedback
                
            }//End loop    
        

        }//End main

    }
}
